import math
import os
import time

import halcon as ha
from util_3d  import *
import cv2
from halcon.numpy_interop import himage_as_numpy_array
# U3V:DA3797248 MV-CU060-10UM  AGV 上的
# U3V:DA3797129 MV-CU060-10UM  机械臂上的
class Camera:

    def __init__(self):
        self.acq_handle = ha.open_framegrabber('MVision', 1, 1, 0, 0, 0, 0, 'progressive', 8, 'default', -1, 'false', 'auto',
                          'U3V:DA3797248 MV-CU060-10UM', 0, -1)
        ha.set_framegrabber_param(self.acq_handle, 'AcquisitionFrameRate', 10.0)
        ha.set_framegrabber_param(self.acq_handle, 'AcquisitionFrameRateEnable', 1)
        self.set_param()
        self.set_param_outer('param_outer_agv.txt')
        ha.grab_image_start(self.acq_handle, -1)
        self.Image = ha.grab_image_async(self.acq_handle, -1)
        Width, Height = ha.get_image_size(self.Image)
        # self.window_image = self.open_window(Width[0]/4, Height[0]/4)
        # ha.disp_obj(self.Image, self.window_image)
        self.method = 'MVision'
        self.name = 'U3V:DA3797248 MV-CU060-10UM'

    def __init__(self, camera_name, method='MVision'):
        self.acq_handle = ha.open_framegrabber(method, 1, 1, 0, 0, 0, 0, 'progressive', 8, 'default', -1, 'false', 'auto',
                                               camera_name, 0, -1)
        self.method = method
        self.name = camera_name
        ha.set_framegrabber_param(self.acq_handle, 'AcquisitionFrameRate', 10.0)
        ha.set_framegrabber_param(self.acq_handle, 'AcquisitionFrameRateEnable', 1)

        ha.grab_image_start(self.acq_handle, -1)
        self.Image = ha.grab_image_async(self.acq_handle, -1)
        self.set_param_inner('./config/param_inner.cal')
        self.set_param_outer('./config/param_outer.txt')
        self.set_param_calib()
        self.width, self.height = ha.get_image_size(self.Image)
        self.update_image_loop_flag = True
        # self.window_image = self.open_window(self.width[0]/4, self.height[0]/4)
        # ha.disp_obj(self.Image, self.window_image)


    def update_image(self, disp_flag = False):
        try:
            self.Image = ha.grab_image(self.acq_handle)
            update_flag = True
        except:
           ha.close_framegrabber(self.acq_handle)
           update_flag = False
           try:
               self.acq_handle = ha.open_framegrabber(self.method, 1, 1, 0, 0, 0, 0, 'progressive', 8, 'default', -1, 'false', 'auto',
                                                      self.name, 0, -1)
               ha.set_framegrabber_param(self.acq_handle, 'AcquisitionFrameRate', 10.0)
               ha.set_framegrabber_param(self.acq_handle, 'AcquisitionFrameRateEnable', 1)
               ha.grab_image_start(self.acq_handle, -1)
           except:
               print('error open camera')

        if disp_flag == True:
            ha.disp_obj(self.Image, self.window_image)
        return self.Image, update_flag

    def get_image(self):
        return self.Image

    def update_image_loop(self, display_flag = False):

        if display_flag:
            self.window_image = self.open_window(self.width[0]/2, self.height[0]/2)
        while self.update_image_loop_flag:
            try:
                self.Image = ha.grab_image(self.acq_handle)
                if display_flag:
                    ha.disp_obj(self.Image, self.window_image)
            except:
                ha.close_framegrabber(self.acq_handle)
                try:
                    self.acq_handle = ha.open_framegrabber(self.method, 1, 1, 0, 0, 0, 0, 'progressive', 8, 'default', -1, 'false', 'auto',
                                                           self.name, 0, -1)
                    ha.set_framegrabber_param(self.acq_handle, 'AcquisitionFrameRate', 10.0)
                    ha.set_framegrabber_param(self.acq_handle, 'AcquisitionFrameRateEnable', 1)
                    ha.grab_image_start(self.acq_handle, -1)
                except:
                    print('error open camera fail')

    def open_window(self, width, height, row=0, col=0, win_id=0):
        if os.name == 'nt':
            ha.set_system('use_window_thread', 'true')
        window = ha.open_window(
            row=row,
            column=col,
            width=width,
            height=height,
            father_window=win_id,
            mode='visible',
            machine=''
        )
        ha.set_draw(window, 'fill')
        ha.set_line_width(window, 2)
        ha.set_color(window, 'green')
        return window

    def set_param(self):
        self.CamInnerParam = ha.read_cam_par('./param_inner_2024_9_22.cal')
        self.CaltabName = 'caltab_30mm.descr'
        self.CalibDataID = ha.create_calib_data('calibration_object', 1, 1)
        ha.set_calib_data_cam_param(self.CalibDataID, 0, [], self.CamInnerParam)
        ha.set_calib_data_calib_object(self.CalibDataID, 0, self.CaltabName)
        ha.set_calib_data(self.CalibDataID, 'model', 'general', 'optimization_method', 'nonlinear')
        # self.template_model_id  = ha.read_shape_model ('./matching_circle1.shm')
        pass
    def set_param_inner(self, inner_param_path):
        self.CamInnerParam = ha.read_cam_par(inner_param_path)

    def set_param_outer(self, file_path):
        content = []
        with open(file_path, 'r') as f:
            for line in f:
                str_list = line.split()
                for str in str_list:
                    content.append(float(str))
        self.out_param_halcon_type = content
        #     # [-0.128855, 0.0288219, -0.0601685, 89.6343, 0.575319, 181.158, 2]  # calib2
    def set_math_template(self):
        self.template_model_id  = ha.read_shape_model ('./matching_circle1.shm')


    def set_param_calib(self, param=None):
        self.CaltabName = './config/caltab_HC050_25.descr'
        self.CalibDataID = ha.create_calib_data('calibration_object', 1, 1)
        ha.set_calib_data_cam_param(self.CalibDataID, 0, [], self.CamInnerParam)
        ha.set_calib_data_calib_object(self.CalibDataID, 0, self.CaltabName)
        ha.set_calib_data(self.CalibDataID, 'model', 'general', 'optimization_method', 'nonlinear')


    def gen_calib_pos(self):
        try:
            ha.find_calib_object(self.Image, self.CalibDataID, 0, 0, 1, [], [])
            Caltab = ha.get_calib_data_observ_contours(self.CalibDataID, 'caltab', 0, 0, 1)
            RCoord, CCoord, Index, self.PoseForCalibrationPlate = ha.get_calib_data_observ_points(self.CalibDataID, 0, 0, 1)
            # ha.set_color(self.window_image, 'green')
            ha.disp_caltab(self.window_image, self.CaltabName, self.CamInnerParam, self.PoseForCalibrationPlate, 1)
            print(self.PoseForCalibrationPlate)
            return True, self.PoseForCalibrationPlate
        except:
            print('error')
            return False, self.PoseForCalibrationPlate


    def gen_cam_t_calib(self, type):
        self.PoseForCalibrationPlate = xyzrpy_to_T(0,0,0,0,0,0)
        for i in range(5):
            try:
                ha.find_calib_object(self.Image, self.CalibDataID, 0, 0, 1, 'alpha', 0.5)
                Caltab = ha.get_calib_data_observ_contours(self.CalibDataID, 'caltab', 0, 0, 1)
                RCoord, CCoord, Index, self.PoseForCalibrationPlate = ha.get_calib_data_observ_points(self.CalibDataID, 0, 0, 1)
                # ha.set_color(self.window_image, 'green')
                # ha.disp_caltab(self.window_image, self.CaltabName, self.CamInnerParam, self.PoseForCalibrationPlate, 1)
                if type == "halcon":
                    return True, self.PoseForCalibrationPlate
                else:
                    return True, halcon_pose_to_T(self.PoseForCalibrationPlate)
            except:
                if i == 4:
                    print('error')
                    return False, self.PoseForCalibrationPlate

    def gen_cam_t_calib_out(self, base_t_end_rokae_type):
        self.PoseForCalibrationPlate = xyzrpy_to_T(0, 0, 0, 0, 0, 0)
        for i in range(10):
            try:
                ha.find_calib_object(self.Image, self.CalibDataID, 0, 0, 1, 'alpha', 0.5) #0.2
                Caltab = ha.get_calib_data_observ_contours(self.CalibDataID, 'caltab', 0, 0, 1)
                RCoord, CCoord, Index, self.PoseForCalibrationPlate = ha.get_calib_data_observ_points(self.CalibDataID,
                                                                                                      0, 0, 1)
                # ha.set_color(self.window_image, 'green')
                # ha.disp_caltab(self.window_image, self.CaltabName, self.CamParam, self.PoseForCalibrationPlate, 1)
                T = rokae_pose_to_T(base_t_end_rokae_type) @ halcon_pose_to_T(
                    self.out_param_halcon_type) @ halcon_pose_to_T(self.PoseForCalibrationPlate)
                return True, T_to_rokae_pose(T)
            except:
                continue
        return False, 6*[0]

    def circle_detect_threshold(self):
        regions = ha.threshold(self.Image, 0, 30)
        regions_open = ha.opening_circle (regions, 5.5)
        regions2 = ha.connection (regions_open)
        circles = ha.select_shape (regions2,  ['circularity','area'], 'and', [0.75,15000], [1.0,59999])
        num = ha.count_obj (circles)
        # ha.disp_obj(circles, self.window_image)
        if num ==1:
             area, row, column = ha.area_center (circles)
             if area[0] > 50000:
                 return False, 0, 0
             elif area[0] < 10000:
                 return False, 0, 0
             else:
                 return True, row[0], column[0]
        return False, 0, 0
    def circle_detect(self):
        row, column, angle, scale, score = ha.find_scaled_shape_model(self.Image, self.template_model_id, -1, 1, 0.9, 1.2, 0.85, 1, 0.5, 'least_squares', 0, 0.9, )
        if len(row) == 1:
            if score[0] > 0.90:
                return True, [row[0], column[0], 0, 0, 0, 0]
        return False, 6*[0]


    def circle_detect1_out(self):
        regions = ha.threshold(self.Image, 0, 50)
        regions_open = ha.opening_circle (regions, 5.5)
        regions2 = ha.connection (regions_open)
        circles = ha.select_shape (regions2,  ['circularity', 'area'], 'and', [0.75,15000], [5000,55000])
        num = ha.count_obj (circles)
        # ha.disp_obj(circles, self.window_image)
        if num ==1:
            area, row, column = ha.area_center (circles)
            if area[0] > 50000:
                return False, 6*[0]
            elif area[0] < 10000:
                return False, 6*[0]
            else:
                R = math.sqrt(area[0]/math.pi)
                if 200 > R > 50:
                    return True, [row[0], column[0], R, 0, 0, 0]
                else:
                    return False, 6 * [0]
        return False, 6*[0]

    def circle_detect_out(self):
        regions_all = ha.threshold(self.Image, 0, 255)
        mean, deviation = ha.intensity(regions_all, self.Image)
        Image1 = ha.scale_image(self.Image, 30 / mean[0], 0)
        Image2 = ha.equ_histo_image(Image1)
        regions = ha.threshold(Image2, 0, 170)
        regions_open = ha.opening_circle(regions, 5)
        regions2 = ha.connection(regions_open)

        selected_regions_area = ha.select_shape(regions2, 'area', 'and', 10000, 29999)
        circles =  ha.select_shape (selected_regions_area, 'circularity', 'and', 0.6, 0.99)
        num = ha.count_obj(circles)
        # ha.disp_obj(circles, self.window_image)
        if num == 1:
            area, row, column = ha.area_center(circles)
            if area[0] > 50000:
                return False, 6 * [0]
            elif area[0] < 10000:
                return False, 6 * [0]
            else:
                R = math.sqrt(area[0] / math.pi)
                if 200 > R > 50:
                    return True, [row[0], column[0], R, 0, 0, 0]
                else:
                    return False, 6 * [0]
        return False, 6 * [0]
    def read_opencv_image(self):
        # self.update_image(False)
        res = himage_as_numpy_array(self.Image)
        img = cv2.cvtColor(res, cv2.COLOR_RGB2BGR)
        img = cv2.resize(img, (800, 600))
        return img
    def close(self):
        self.update_image_loop_flag = False
        time.sleep(0.5)
        ha.close_framegrabber(self.acq_handle)

if __name__ == '__main__':
    # U3V:DA3797248 MV-CU060-10UM
    # U3V:DA3797129 MV-CU060-10UM
    ca = Camera(camera_name='U3V:DA3797129 MV-CU060-10UM')
    ca.set_param_outer('param_outer_agv.txt')
    # ca.set_param()
    while True:
        img, update_flag = ca.update_image(True)
        # ca.circle_detect_template_match()
        flag ,resault = ca.gen_cam_t_calib_out([0,0,0,0,0,0])
        print(flag)
        # ca.gen_calib_pos()

